/*
	This file is part of Floculate.
	Copyright (C) 2008  Bill Whitacre

	Floculate is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	Floculate is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "math.h"

RatMathMatrix4 *RatMathMatrix4Create()
{
	RatMathMatrix4 *mat=(RatMathMatrix4 *)malloc(sizeof(RatMathMatrix4));
	memset(mat,0,sizeof(RatMathMatrix4));

	return mat;
}

RatMathMatrix4 *RatMathMatrix4CreateFromQuaternion(RatMathQuaternion *quat)
{
	float xx=quat->x*quat->x,yy=quat->y*quat->y,zz=quat->z*quat->z;
	float xy=quat->x*quat->y,xz=quat->x*quat->z,yz=quat->y*quat->z;
	float wx=quat->w*quat->x,wy=quat->w*quat->y,wz=quat->w*quat->z;

	RatMathMatrix4 *newmat=(RatMathMatrix4 *)malloc(sizeof(RatMathMatrix4));

	RatMathMatrix4WriteIdentity(newmat);
	newmat->mat00=1.0f-2.0f*(yy+zz);
	newmat->mat01=2.0f*(xy-wz);
	newmat->mat02=2.0f*(xz+wy);
	newmat->mat10=2.0f*(xy+wz);
	newmat->mat11=1.0f-2.0f*(xx+zz);
	newmat->mat12=2.0f*(yz-wx);
	newmat->mat20=2.0f*(xz-wy);
	newmat->mat21=2.0f*(yz+wx);
	newmat->mat22=1.0f-2.0f*(xx+yy);
	newmat->mat33=1.0f;

	return newmat;
}

void RatMathMatrix4WriteIdentity(RatMathMatrix4 *mat)
{
	mat->mat00=1;
	mat->mat01=0;
	mat->mat02=0;
	mat->mat03=0;
	mat->mat10=0;
	mat->mat11=1;
	mat->mat12=0;
	mat->mat13=0;
	mat->mat20=0;
	mat->mat21=0;
	mat->mat22=1;
	mat->mat23=0;
	mat->mat30=0;
	mat->mat31=0;
	mat->mat32=0;
	mat->mat33=1;
}

void RatMathMatrix4WriteIdentity3(RatMathMatrix4 *mat)
{
	mat->mat00=1;
	mat->mat01=0;
	mat->mat02=0;
	mat->mat10=0;
	mat->mat11=1;
	mat->mat12=0;
	mat->mat20=0;
	mat->mat21=0;
	mat->mat22=1;
}

void RatMathMatrix4WriteZeroes(RatMathMatrix4 *mat)
{
	mat->mat00=0;
	mat->mat01=0;
	mat->mat02=0;
	mat->mat03=0;
	mat->mat10=0;
	mat->mat11=0;
	mat->mat12=0;
	mat->mat13=0;
	mat->mat20=0;
	mat->mat21=0;
	mat->mat22=0;
	mat->mat23=0;
	mat->mat30=0;
	mat->mat31=0;
	mat->mat32=0;
	mat->mat33=0;
}

void RatMathMatrix4WriteOnes(RatMathMatrix4 *mat)
{
	mat->mat00=1;
	mat->mat01=1;
	mat->mat02=1;
	mat->mat03=1;
	mat->mat10=1;
	mat->mat11=1;
	mat->mat12=1;
	mat->mat13=1;
	mat->mat20=1;
	mat->mat21=1;
	mat->mat22=1;
	mat->mat23=1;
	mat->mat30=1;
	mat->mat31=1;
	mat->mat32=1;
	mat->mat33=1;
}

void RatMathMatrix4Copy(RatMathMatrix4 *mat0,RatMathMatrix4 *mat1)
{
	mat0->mat00=mat1->mat00;
	mat0->mat01=mat1->mat01;
	mat0->mat02=mat1->mat02;
	mat0->mat03=mat1->mat03;

	mat0->mat10=mat1->mat10;
	mat0->mat11=mat1->mat11;
	mat0->mat12=mat1->mat12;
	mat0->mat13=mat1->mat13;

	mat0->mat20=mat1->mat20;
	mat0->mat21=mat1->mat21;
	mat0->mat22=mat1->mat22;
	mat0->mat23=mat1->mat23;

	mat0->mat30=mat1->mat30;
	mat0->mat31=mat1->mat31;
	mat0->mat32=mat1->mat32;
	mat0->mat33=mat1->mat33;
}

RatMathMatrix4 *RatMathMatrix4Clone(RatMathMatrix4 *mat)
{
	RatMathMatrix4 *newmat=(RatMathMatrix4 *)malloc(sizeof(RatMathMatrix4));

	newmat->mat00=mat->mat00;
	newmat->mat01=mat->mat01;
	newmat->mat02=mat->mat02;
	newmat->mat03=mat->mat03;

	newmat->mat10=mat->mat10;
	newmat->mat11=mat->mat11;
	newmat->mat12=mat->mat12;
	newmat->mat13=mat->mat13;

	newmat->mat20=mat->mat20;
	newmat->mat21=mat->mat21;
	newmat->mat22=mat->mat22;
	newmat->mat23=mat->mat23;

	newmat->mat30=mat->mat30;
	newmat->mat31=mat->mat31;
	newmat->mat32=mat->mat32;
	newmat->mat33=mat->mat33;

	return newmat;
}

void RatMathMatrix4ExtractUpVector(RatMathMatrix4 *mat,RatMathVector3 *up)
{
	up->x=mat->mat10;
	up->y=mat->mat11;
	up->z=mat->mat12;
}

void RatMathMatrix4ExtractRightVector(RatMathMatrix4 *mat,RatMathVector3 *rt)
{
	rt->x=mat->mat00;
	rt->y=mat->mat01;
	rt->z=mat->mat02;
}

void RatMathMatrix4ExtractLookVector(RatMathMatrix4 *mat,RatMathVector3 *look)
{
	look->x=mat->mat20;
	look->y=mat->mat21;
	look->z=mat->mat22;
}

void RatMathMatrix4ExtractPosVector(RatMathMatrix4 *mat,RatMathVector3 *pos)
{
	pos->x=mat->mat30;
	pos->y=mat->mat31;
	pos->z=mat->mat32;
}

void RatMathMatrix4WriteUpVector(RatMathMatrix4 *mat,RatMathVector3 *up)
{
	mat->mat10=up->x;
	mat->mat11=up->y;
	mat->mat12=up->z;
}

void RatMathMatrix4WriteRightVector(RatMathMatrix4 *mat,RatMathVector3 *rt)
{
	mat->mat00=rt->x;
	mat->mat01=rt->y;
	mat->mat02=rt->z;
}

void RatMathMatrix4WriteLookVector(RatMathMatrix4 *mat,RatMathVector3 *look)
{
	mat->mat20=look->x;
	mat->mat21=look->y;
	mat->mat22=look->z;
}

void RatMathMatrix4WritePosVector(RatMathMatrix4 *mat,RatMathVector3 *pos)
{
	mat->mat30=pos->x;
	mat->mat31=pos->y;
	mat->mat32=pos->z;
}


void RatMathMatrix4Invert(RatMathMatrix4 *mat)
{
	float c00,c10,c20,c30;
	float c01,c11,c21,c31;
	float c02,c12,c22,c32;
	float c03,c13,c23,c33;

	float m00,m10,m20,m30;
	float m01,m11,m21,m31;
	float m02,m12,m22,m32;
	float m03,m13,m23,m33;

	float determ,invdeterm;

	m00=mat->mat00,m10=mat->mat10,m20=mat->mat20,m30=mat->mat30;
	m01=mat->mat01,m11=mat->mat11,m21=mat->mat21,m31=mat->mat31;
	m02=mat->mat02,m12=mat->mat12,m22=mat->mat22,m32=mat->mat32;
	m03=mat->mat03,m13=mat->mat13,m23=mat->mat23,m33=mat->mat33;

	c00=m12*m23*m31-m13*m22*m31+m13*m21*m32-m11*m23*m32-m12*m21*m33+m11*m22*m33;
	c01=m03*m22*m31-m02*m23*m31-m03*m21*m32+m01*m23*m32+m02*m21*m33-m01*m22*m33;
	c02=m02*m13*m31-m03*m12*m31+m03*m11*m32-m01*m13*m32-m02*m11*m33+m01*m12*m33;
	c03=m03*m12*m21-m02*m13*m21-m03*m11*m22+m01*m13*m22+m02*m11*m23-m01*m12*m23;

	determ=m00*c00-m01*c01+m02*c02-m03*c03;
	if (determ==0.0f)
	{
		RatMathMatrix4WriteIdentity(mat);
		return;
	}
	invdeterm=1.0f/determ;

	c10=m13*m22*m30-m12*m23*m30-m13*m20*m32+m10*m23*m32+m12*m20*m33-m10*m22*m33;
	c11=m02*m23*m30-m03*m22*m30+m03*m20*m32-m00*m23*m32-m02*m20*m33+m00*m22*m33;
	c12=m03*m12*m30-m02*m13*m30-m03*m10*m32+m00*m13*m32+m02*m10*m33-m00*m12*m33;
	c13=m02*m13*m20-m03*m12*m20+m03*m10*m22-m00*m13*m22-m02*m10*m23+m00*m12*m23;
	c20=m11*m23*m30-m13*m21*m30+m13*m20*m31-m10*m23*m31-m11*m20*m33+m10*m21*m33;
	c21=m03*m21*m30-m01*m23*m30-m03*m20*m31+m00*m23*m31+m01*m20*m33-m00*m21*m33;
	c22=m01*m13*m30-m03*m11*m30+m03*m10*m31-m00*m13*m31-m01*m10*m33+m00*m11*m33;
	c23=m03*m11*m20-m01*m13*m20-m03*m10*m21+m00*m13*m21+m01*m10*m23-m00*m11*m23;
	c30=m12*m21*m30-m11*m22*m30-m12*m20*m31+m10*m22*m31+m11*m20*m32-m10*m21*m32;
	c31=m01*m22*m30-m02*m21*m30+m02*m20*m31-m00*m22*m31-m01*m20*m32+m00*m21*m32;
	c32=m02*m11*m30-m01*m12*m30-m02*m10*m31+m00*m12*m31+m01*m10*m32-m00*m11*m32;
	c33=m01*m12*m20-m02*m11*m20+m02*m10*m21-m00*m12*m21-m01*m10*m22+m00*m11*m22;

	mat->mat00= c00*invdeterm;
	mat->mat10=-c10*invdeterm;
	mat->mat20= c20*invdeterm;
	mat->mat30=-c30*invdeterm;
	mat->mat01=-c01*invdeterm;
	mat->mat11= c11*invdeterm;
	mat->mat21=-c21*invdeterm;
	mat->mat31= c31*invdeterm;
	mat->mat02= c02*invdeterm;
	mat->mat12=-c12*invdeterm;
	mat->mat22= c22*invdeterm;
	mat->mat32=-c32*invdeterm;
	mat->mat03=-c03*invdeterm;
	mat->mat13= c13*invdeterm;
	mat->mat23=-c23*invdeterm;
	mat->mat33= c33*invdeterm;
}

void RatMathMatrix4Transpose(RatMathMatrix4 *mat)
{
	float m00=mat->mat00,m10=mat->mat10,m20=mat->mat20,m30=mat->mat30;
	float m01=mat->mat01,m11=mat->mat11,m21=mat->mat21,m31=mat->mat31;
	float m02=mat->mat02,m12=mat->mat12,m22=mat->mat22,m32=mat->mat32;
	float m03=mat->mat03,m13=mat->mat13,m23=mat->mat23,m33=mat->mat33;

	mat->mat00=m00;
	mat->mat01=m10;
	mat->mat02=m20;
	mat->mat03=m30;

	mat->mat10=m01;
	mat->mat11=m11;
	mat->mat12=m21;
	mat->mat13=m31;

	mat->mat20=m02;
	mat->mat21=m12;
	mat->mat22=m22;
	mat->mat23=m32;

	mat->mat30=m03;
	mat->mat31=m13;
	mat->mat32=m23;
	mat->mat33=m33;
}

void RatMathMatrix4Mult(RatMathMatrix4 *mat0,RatMathMatrix4 *mat1)
{
	float m00=mat0->mat00*mat1->mat00+mat0->mat10*mat1->mat01+mat0->mat20*mat1->mat02+mat0->mat30*mat1->mat03;
	float m01=mat0->mat01*mat1->mat00+mat0->mat11*mat1->mat01+mat0->mat21*mat1->mat02+mat0->mat31*mat1->mat03;
	float m02=mat0->mat02*mat1->mat00+mat0->mat12*mat1->mat01+mat0->mat22*mat1->mat02+mat0->mat32*mat1->mat03;
	float m03=mat0->mat03*mat1->mat00+mat0->mat13*mat1->mat01+mat0->mat23*mat1->mat02+mat0->mat33*mat1->mat03;

	float m10=mat0->mat00*mat1->mat10+mat0->mat10*mat1->mat11+mat0->mat20*mat1->mat12+mat0->mat30*mat1->mat13;
	float m11=mat0->mat01*mat1->mat10+mat0->mat11*mat1->mat11+mat0->mat21*mat1->mat12+mat0->mat31*mat1->mat13;
	float m12=mat0->mat02*mat1->mat10+mat0->mat12*mat1->mat11+mat0->mat22*mat1->mat12+mat0->mat32*mat1->mat13;
	float m13=mat0->mat03*mat1->mat10+mat0->mat13*mat1->mat11+mat0->mat23*mat1->mat12+mat0->mat33*mat1->mat13;

	float m20=mat0->mat00*mat1->mat20+mat0->mat10*mat1->mat21+mat0->mat20*mat1->mat22+mat0->mat30*mat1->mat23;
	float m21=mat0->mat01*mat1->mat20+mat0->mat11*mat1->mat21+mat0->mat21*mat1->mat22+mat0->mat31*mat1->mat23;
	float m22=mat0->mat02*mat1->mat20+mat0->mat12*mat1->mat21+mat0->mat22*mat1->mat22+mat0->mat32*mat1->mat23;
	float m23=mat0->mat03*mat1->mat20+mat0->mat13*mat1->mat21+mat0->mat23*mat1->mat22+mat0->mat33*mat1->mat23;

	float m30=mat0->mat00*mat1->mat30+mat0->mat10*mat1->mat31+mat0->mat20*mat1->mat32+mat0->mat30*mat1->mat33;
	float m31=mat0->mat01*mat1->mat30+mat0->mat11*mat1->mat31+mat0->mat21*mat1->mat32+mat0->mat31*mat1->mat33;
	float m32=mat0->mat02*mat1->mat30+mat0->mat12*mat1->mat31+mat0->mat22*mat1->mat32+mat0->mat32*mat1->mat33;
	float m33=mat0->mat03*mat1->mat30+mat0->mat13*mat1->mat31+mat0->mat23*mat1->mat32+mat0->mat33*mat1->mat33;

	mat0->mat00=m00;
	mat0->mat01=m01;
	mat0->mat02=m02;
	mat0->mat03=m03;
	mat0->mat10=m10;
	mat0->mat11=m11;
	mat0->mat12=m12;
	mat0->mat13=m13;
	mat0->mat20=m20;
	mat0->mat21=m21;
	mat0->mat22=m22;
	mat0->mat23=m23;
	mat0->mat30=m30;
	mat0->mat31=m31;
	mat0->mat32=m32;
	mat0->mat33=m33;
}

void RatMathMatrix4Translate(RatMathMatrix4 *mat,RatMathVector3 *vec)
{
	mat->mat30=mat->mat00*vec->x+mat->mat10*vec->y+mat->mat20*vec->z+mat->mat30;
	mat->mat31=mat->mat01*vec->x+mat->mat11*vec->y+mat->mat21*vec->z+mat->mat31;
	mat->mat32=mat->mat02*vec->x+mat->mat12*vec->y+mat->mat22*vec->z+mat->mat32;
}

void RatMathMatrix4Scale(RatMathMatrix4 *mat,RatMathVector3 *vec)
{
	mat->mat00*=vec->x;
	mat->mat01*=vec->x;
	mat->mat02*=vec->x;

	mat->mat10*=vec->y;
	mat->mat11*=vec->y;
	mat->mat12*=vec->y;

	mat->mat20*=vec->z;
	mat->mat21*=vec->z;
	mat->mat22*=vec->z;
}

void RatMathMatrix4Rotate(RatMathMatrix4 *mat,RatMathEuler *eul)
{
	float cos_ang,sin_ang;
	float m00,m10;
	float m01,m11;
	float m02,m12;

	/* do yaw */
	cos_ang=cosf(eul->y);
	sin_ang=sinf(eul->y);

	m00=mat->mat00*cos_ang+mat->mat20*-sin_ang;
	m01=mat->mat01*cos_ang+mat->mat21*-sin_ang;
	m02=mat->mat02*cos_ang+mat->mat22*-sin_ang;

	mat->mat20=mat->mat00*sin_ang+mat->mat20*cos_ang;
	mat->mat21=mat->mat01*sin_ang+mat->mat21*cos_ang;
	mat->mat22=mat->mat02*sin_ang+mat->mat22*cos_ang;

	mat->mat00=m00;
	mat->mat01=m01;
	mat->mat02=m02;

	/* do pitch */
	cos_ang=cosf(eul->x);
	sin_ang=sinf(eul->x);

	m10=mat->mat10*cos_ang+mat->mat20*sin_ang;
	m11=mat->mat11*cos_ang+mat->mat21*sin_ang;
	m12=mat->mat12*cos_ang+mat->mat22*sin_ang;

	mat->mat20=mat->mat10*-sin_ang+mat->mat20*cos_ang;
	mat->mat21=mat->mat11*-sin_ang+mat->mat21*cos_ang;
	mat->mat22=mat->mat12*-sin_ang+mat->mat22*cos_ang;

	mat->mat10=m10;
	mat->mat11=m11;
	mat->mat12=m12;

	/* do roll */
	cos_ang=cosf(eul->z);
	sin_ang=sinf(eul->z);

	m00=mat->mat00*cos_ang+mat->mat10*sin_ang;
	m01=mat->mat01*cos_ang+mat->mat11*sin_ang;
	m02=mat->mat02*cos_ang+mat->mat12*sin_ang;

	mat->mat10=mat->mat00*-sin_ang+mat->mat10*cos_ang;
	mat->mat11=mat->mat01*-sin_ang+mat->mat11*cos_ang;
	mat->mat12=mat->mat02*-sin_ang+mat->mat12*cos_ang;

	mat->mat00=m00;
	mat->mat01=m01;
	mat->mat02=m02;
}

void RatMathMatrix4WriteFrustum(RatMathMatrix4 *mat,RatMathFrustum *frustum)
{
	mat->mat00=(2*frustum->pnear)/(frustum->pright-frustum->pleft);
	mat->mat01=0;
	mat->mat02=(frustum->pright+frustum->pleft)/(frustum->pright-frustum->pleft);
	mat->mat03=0;

	mat->mat10=0;
	mat->mat11=(2*frustum->pnear)/(frustum->ptop-frustum->pbottom);
	mat->mat12=(frustum->ptop+frustum->pbottom)/(frustum->ptop-frustum->pbottom);
	mat->mat13=0;

	mat->mat20=0;
	mat->mat21=0;
	mat->mat22=-((frustum->pfar+frustum->pnear)/(frustum->pfar-frustum->pnear));
	mat->mat23=-((2*frustum->pfar*frustum->pnear)/(frustum->pfar-frustum->pnear));

	mat->mat30=0;
	mat->mat31=0;
	mat->mat32=-1;
	mat->mat33=0;
}

void RatMathMatrix4MultFrustum(RatMathMatrix4 *mat,RatMathFrustum *frustum)
{
	RatMathMatrix4 frmat;

	frmat.mat00=(2*frustum->pnear)/(frustum->pright-frustum->pleft);
	frmat.mat01=0;
	frmat.mat02=(frustum->pright+frustum->pleft)/(frustum->pright-frustum->pleft);
	frmat.mat03=0;

	frmat.mat10=0;
	frmat.mat11=(2*frustum->pnear)/(frustum->ptop-frustum->pbottom);
	frmat.mat12=(frustum->ptop+frustum->pbottom)/(frustum->ptop-frustum->pbottom);
	frmat.mat13=0;

	frmat.mat20=0;
	frmat.mat21=0;
	frmat.mat22=-((frustum->pfar+frustum->pnear)/(frustum->pfar-frustum->pnear));
	frmat.mat23=-((2*frustum->pfar*frustum->pnear)/(frustum->pfar-frustum->pnear));

	frmat.mat30=0;
	frmat.mat31=0;
	frmat.mat32=-1;
	frmat.mat33=0;

	RatMathMatrix4Mult(mat,&frmat);
}

void RatMathMatrix4GetArray(RatMathMatrix4 *mat,float newmat[16])
{
	newmat[ 0]=mat->mat00;
	newmat[ 1]=mat->mat10;
	newmat[ 2]=mat->mat20;
	newmat[ 3]=mat->mat30;

	newmat[ 4]=mat->mat01;
	newmat[ 5]=mat->mat11;
	newmat[ 6]=mat->mat21;
	newmat[ 7]=mat->mat31;

	newmat[ 8]=mat->mat02;
	newmat[ 9]=mat->mat12;
	newmat[10]=mat->mat22;
	newmat[11]=mat->mat32;

	newmat[12]=mat->mat03;
	newmat[13]=mat->mat13;
	newmat[14]=mat->mat23;
	newmat[15]=mat->mat33;
}

void RatMathMatrix4GetTransposeArray(RatMathMatrix4 *mat,float newmat[16])
{
	newmat[ 0]=mat->mat00;
	newmat[ 1]=mat->mat01;
	newmat[ 2]=mat->mat02;
	newmat[ 3]=mat->mat03;

	newmat[ 4]=mat->mat10;
	newmat[ 5]=mat->mat11;
	newmat[ 6]=mat->mat12;
	newmat[ 7]=mat->mat13;

	newmat[ 8]=mat->mat20;
	newmat[ 9]=mat->mat21;
	newmat[10]=mat->mat22;
	newmat[11]=mat->mat23;

	newmat[12]=mat->mat30;
	newmat[13]=mat->mat31;
	newmat[14]=mat->mat32;
	newmat[15]=mat->mat33;
}

RatMathVector3 *RatMathVector3Create()
{
	RatMathVector3 *vec=(RatMathVector3 *)malloc(sizeof(RatMathVector3));
	memset(vec,0,sizeof(RatMathVector3));
	return vec;
}

RatMathVector3 *RatMathVector3CreateFromQuaternion(RatMathQuaternion *quat)
{
	RatMathVector3 *vec=(RatMathVector3 *)malloc(sizeof(RatMathVector3));

	vec->x=quat->x;
	vec->y=quat->y;
	vec->z=quat->z;

	return vec;
}

void RatMathVector3WriteZeroes(RatMathVector3 *vec)
{
	memset(vec,0,sizeof(RatMathVector3));
}

void RatMathVector3Copy(RatMathVector3 *vec0,RatMathVector3 *vec1)
{
	vec0->x=vec1->x;
	vec0->y=vec1->y;
	vec0->z=vec1->z;
}

RatMathVector3 *RatMathVector3Clone(RatMathVector3 *vec)
{
	RatMathVector3 *newvec=(RatMathVector3 *)malloc(sizeof(RatMathVector3));

	newvec->x=vec->x;
	newvec->y=vec->y;
	newvec->z=vec->z;

	return newvec;
}

void RatMathVector3Add(RatMathVector3 *vec0,RatMathVector3 *vec1)
{
	vec0->x+=vec1->x;
	vec0->y+=vec1->y;
	vec0->z+=vec1->z;
}

void RatMathVector3Sub(RatMathVector3 *vec0,RatMathVector3 *vec1)
{
	vec0->x-=vec1->x;
	vec0->y-=vec1->y;
	vec0->z-=vec1->z;
}

void RatMathVector3Mult(RatMathVector3 *vec,float val)
{
	vec->x*=val;
	vec->y*=val;
	vec->z*=val;
}

void RatMathVector3MultQuaternion(RatMathVector3 *vec,RatMathQuaternion *quat)
{
	RatMathQuaternion tempquat,vecquat,resquat,*conj;
	vecquat.x=vec->x;
	vecquat.y=vec->y;
	vecquat.z=vec->z;
	vecquat.w=0.0f;
	 
	conj=RatMathQuaternionGetConjugate(quat);
	memcpy(&resquat,&vecquat,sizeof(RatMathQuaternion));
	RatMathQuaternionMult(&resquat,conj);
	memcpy(&tempquat,&resquat,sizeof(RatMathQuaternion));
	memcpy(&resquat,quat,sizeof(RatMathQuaternion));
	RatMathQuaternionMult(&resquat,&tempquat);
	 	
	free((void *)conj);
	vec->x=resquat.x;
	vec->y=resquat.y;
	vec->z=resquat.z;
}

void RatMathVector3Div(RatMathVector3 *vec,float val)
{
	vec->x/=val;
	vec->y/=val;
	vec->z/=val;
}

float RatMathVector3Dot(RatMathVector3 *vec0,RatMathVector3 *vec1)
{
	return vec0->x*vec1->x+vec0->y*vec1->y+vec0->z*vec1->z;
}

RatMathVector3 *RatMathVector3Cross(RatMathVector3 *vec0,RatMathVector3 *vec1)
{
	RatMathVector3 *newvec=(RatMathVector3 *)malloc(sizeof(RatMathVector3));

	newvec->x=vec0->y*vec1->z-vec0->z*vec1->y;
	newvec->y=vec0->z*vec1->x-vec0->x*vec1->z;
	newvec->z=vec0->x*vec1->y-vec0->y*vec1->x;

	return newvec;
}

void RatMathVector3Normalize(RatMathVector3 *vec)
{
	register float d=1.0f/sqrtf(vec->x*vec->x+vec->y*vec->y+vec->z*vec->z);

	vec->x*=d;
	vec->y*=d;
	vec->z*=d;
}

void RatMathVector3SetMagnitude(RatMathVector3 *vec,float mag)
{
	register float d=1.0f/sqrtf(vec->x*vec->x+vec->y*vec->y+vec->z*vec->z);

	vec->x*=d*mag;
	vec->y*=d*mag;
	vec->z*=d*mag;
}

float RatMathVector3GetMagnitude(RatMathVector3 *vec)
{
	return sqrt(vec->x*vec->x+vec->y*vec->y+vec->z*vec->z);
}

float RatMathVector3GetMagSquared(RatMathVector3 *vec)
{
	return vec->x*vec->x+vec->y*vec->y+vec->z*vec->z;
}

float RatMathVector3GetPitch(RatMathVector3 *vec)
{
	return atan2f(sqrtf(vec->x*vec->x+vec->z*vec->z),vec->y)-MATH_PI/2.0f;
}

float RatMathVector3GetYaw(RatMathVector3 *vec)
{
	return atan2f(-vec->x,vec->z);
}


RatMathEuler *RatMathEulerCreateFromMatrix4(RatMathMatrix4 *mat)
{
	RatMathEuler *neweul=(RatMathEuler *)malloc(sizeof(RatMathEuler));

	neweul->x=atan2f(mat->mat21,sqrt(mat->mat20*mat->mat20+mat->mat22*mat->mat22));
	neweul->y=atan2f(mat->mat20,mat->mat22);
	neweul->z=atan2f(mat->mat01,mat->mat11);

	return neweul;
}

RatMathEuler *RatMathEulerCreateFromVector3(RatMathVector3 *vec)
{
	RatMathEuler *neweul=(RatMathEuler *)malloc(sizeof(RatMathEuler));

	neweul->x=atan2f(sqrtf(vec->x*vec->x+vec->z*vec->z),vec->y)-MATH_PI/2.0f;
	neweul->y=atan2f(-vec->x,vec->z);
	neweul->z=0;

	return neweul;
}

RatMathEuler *RatMathEulerCreateFromVector3AndRoll(RatMathVector3 *vec,float roll)
{
	RatMathEuler *neweul=(RatMathEuler *)malloc(sizeof(RatMathEuler));

	neweul->x=atan2f(sqrtf(vec->x*vec->x+vec->z*vec->z),vec->y)-MATH_PI/2.0f;
	neweul->y=atan2f(-vec->x,vec->z);
	neweul->z=roll;

	return neweul;
}

RatMathEuler *RatMathEulerCreateFromQuaternion(RatMathQuaternion *quat)
{
	RatMathMatrix4 *mat=RatMathMatrix4CreateFromQuaternion(quat);
	RatMathEuler *eul=RatMathEulerCreateFromMatrix4(mat);
	free((void *)mat);
	return eul;
}

RatMathQuaternion *RatMathQuaternionCreate()
{
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));
	memset(quat,0,sizeof(RatMathQuaternion));
	return quat;
}

RatMathQuaternion *RatMathQuaternionCreateFromVector3(RatMathVector3 *vec,float w)
{
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	quat->x=vec->x;
	quat->y=vec->y;
	quat->z=vec->z;
	quat->w=w;

	return quat;
}

RatMathQuaternion *RatMathQuaternionCreateFromMatrix4(RatMathMatrix4 *mat)
{
	RatMathEuler *eul=RatMathEulerCreateFromMatrix4(mat);
	RatMathQuaternion *quat=RatMathQuaternionCreateFromEuler(eul);
	free((void *)eul);
	return quat;
}

RatMathQuaternion *RatMathQuaternionCreateFromAxisAngle(RatMathVector3 *axis,float angle)
{
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	float sin_angle;
	RatMathVector3 naxis;
	angle*=0.5f;
	
	memcpy(&naxis,axis,sizeof(RatMathVector3));
	RatMathVector3Normalize(&naxis);
 	sin_angle=sinf(angle);
 
	quat->x=naxis.x*sin_angle;
	quat->y=naxis.y*sin_angle;
	quat->z=naxis.z*sin_angle;
	quat->w=cosf(angle);

	return quat;
}

RatMathQuaternion *RatMathQuaternionCreateFromEuler(RatMathEuler *euler)
{
	float p=euler->y;
	float y=euler->x;
	float r=euler->z;
 
	float sinp=sinf(p);
	float siny=sinf(y);
	float sinr=sinf(r);
	float cosp=cosf(p);
	float cosy=cosf(y);
	float cosr=cosf(r);
	
	RatMathQuaternion *quat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));
 
	quat->x=sinr*cosp*cosy-cosr*sinp*siny;
	quat->y=cosr*sinp*cosy+sinr*cosp*siny;
	quat->z=cosr*cosp*siny-sinr*sinp*cosy;
	quat->w=cosr*cosp*cosy+sinr*sinp*siny;
 
	RatMathQuaternionNormalize(quat);
	
	return quat;
}

void RatMathQuaternionNormalize(RatMathQuaternion *quat)
{
	register float mag2=quat->w*quat->w+quat->x*quat->x+quat->y*quat->y+quat->z*quat->z;
	
	if (fabs(mag2-1.0f))
	{
		register float mag=sqrtf(mag2);
		quat->w/=mag;
		quat->x/=mag;
		quat->y/=mag;
		quat->z/=mag;
	}
}

void RatMathQuaternionWriteIdentity(RatMathQuaternion *quat)
{
	quat->x=0.0f;
	quat->y=0.0f;
	quat->z=0.0f;
	quat->w=1.0f;
}

void RatMathQuaternionMult(RatMathQuaternion *quat0,RatMathQuaternion *quat1)
{
	RatMathQuaternion result;
	
	result.x=quat0->w*quat1->x+quat0->x*quat1->w+quat0->y*quat1->z-quat0->z*quat1->y;
	result.y=quat0->w*quat1->y+quat0->y*quat1->w+quat0->z*quat1->x-quat0->x*quat1->z;
	result.z=quat0->w*quat1->z+quat0->z*quat1->w+quat0->x*quat1->y-quat0->y*quat1->x;
	result.w=quat0->w*quat1->w-quat0->x*quat1->x-quat0->y*quat1->y-quat0->z*quat1->z;
	
	memcpy(quat0,&result,sizeof(RatMathQuaternion));
}

void RatMathQuaternionCopy(RatMathQuaternion *quat0,RatMathQuaternion *quat1,int copy_w)
{
	quat0->x=quat1->x;
	quat0->y=quat1->y;
	quat0->z=quat1->z;
	if (copy_w!=0) quat0->w=quat1->w;
}

RatMathQuaternion *RatMathQuaternionGetConjugate(RatMathQuaternion *quat)
{
	RatMathQuaternion *newquat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	newquat->x=-quat->x;
	newquat->y=-quat->y;
	newquat->z=-quat->z;
	newquat->w=quat->w;
	
	return newquat;
}

RatMathQuaternion *RatMathQuaternionClone(RatMathQuaternion *quat,int clone_w)
{
	RatMathQuaternion *newquat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	newquat->x=quat->x;
	newquat->y=quat->y;
	newquat->z=quat->z;
	if (clone_w!=0) newquat->w=quat->w; else newquat->w=0.0f;

	return newquat;
}

// this is a zero threshold; less than this is interpreted in SLerp as 0
#define EPSILON 0.00001f

RatMathQuaternion *RatMathQuaternionSlerp(RatMathQuaternion *quat0,RatMathQuaternion *quat1,float t)
{
	float cosineom,sineom,scalar_x,scalar_y,scalar_z,scalar_w;
	float omega,scale0,scale1;

	RatMathQuaternion *newquat=(RatMathQuaternion *)malloc(sizeof(RatMathQuaternion));

	if ((fabs(quat0->x-quat1->x)<EPSILON)&&(fabs(quat0->y-quat1->y)<EPSILON)&&
	   (fabs(quat0->z-quat1->z)<EPSILON)&&(fabs(quat0->w-quat1->w)<EPSILON))
	{
		newquat->x=quat1->x;
		newquat->y=quat1->y;
		newquat->z=quat1->z;
		newquat->w=quat1->w;
		return newquat;
	}

	cosineom=quat0->x*quat1->x+quat0->y*quat1->y+quat0->z*quat1->z+quat0->w*quat1->w;

	if (cosineom<=0)
	{
		cosineom=-cosineom;
		scalar_x=-quat1->x;
		scalar_y=-quat1->y;
		scalar_z=-quat1->z;
		scalar_w=-quat1->w;
	}
	else
	{
		scalar_x=quat1->x;
		scalar_y=quat1->y;
		scalar_z=quat1->z;
		scalar_w=quat1->w;
	}

	if ((1.0f-cosineom)>EPSILON) 
	{	// do this whole SLerp thing
		omega=acosf(cosineom);
		sineom=sinf(omega);
		scale0=sinf((1.0f-t)*omega)/sineom;
		scale1=sinf(t*omega)/sineom;
	}
	else
	{	// close enough to do a straight up linear interpolation
		scale0=1.0f-t;
		scale1=t;
	}

	newquat->x=scale0*quat0->x+scale1*scalar_x;
	newquat->y=scale0*quat0->y+scale1*scalar_y;
	newquat->z=scale0*quat0->z+scale1*scalar_z;
	newquat->w=scale0*quat0->w+scale1*scalar_w;

	return newquat;
}

RatMathArcBall *RatMathArcBallCreate(float ex,float wy,float width,float height)
{
	RatMathArcBall *arcball=(RatMathArcBall *)malloc(sizeof(RatMathArcBall));
	
	memset(arcball,0,sizeof(RatMathArcBall));
	arcball->ex=ex;
	arcball->wy=wy;
	arcball->width=width;
	arcball->height=height;
	
	return arcball;
}

void RatMathArcBallMouseDim(RatMathArcBall *arcball,float ex,float wy,float width,float height)
{
	arcball->ex=ex;
	arcball->wy=wy;
	arcball->width=width;
	arcball->height=height;
}

void RatMathArcBallMapSphere(RatMathArcBall *arcball,float x,float y,RatMathVector3 *surfvec)
{
	float lensqr;

	// transform the coordinates to arcball space
	x-=arcball->ex; y-=arcball->wy;
	x=(arcball->width*0.5f-x)/(arcball->width*0.5f);
	y=(y-arcball->height*0.5f)/(arcball->height*0.5f);
	
	// get the distance of the point to the center squared
	lensqr=x*x+y*y;
//	printf("map to sphere: x=%f y=%f lsq=%f\n",x,y,lensqr);
	
	if (lensqr>1.0f)
	{
		// map outside sphere
		float norm;

		// compute normalize coeficient
		norm=1.0f/sqrtf(lensqr);
		
		// set point
		surfvec->x=x*norm;
		surfvec->y=y*norm;
		surfvec->z=0.0f;
	}
	else
	{
		// map on surface of sphere
		surfvec->x=x;
		surfvec->y=y;
		surfvec->z=sqrtf(1.0f-lensqr);
	}
}

void RatMathArcBallGrab(RatMathArcBall *arcball,float x,float y)
{
	RatMathArcBallMapSphere(arcball,x,y,&(arcball->grabvector));
}

void RatMathArcBallDrag(RatMathArcBall *arcball,float x,float y,RatMathQuaternion *rotq)
{
	RatMathArcBallMapSphere(arcball,x,y,&(arcball->dragvector));

	if (rotq)
	{
		RatMathVector3 *perp=RatMathVector3Cross(&(arcball->grabvector),&(arcball->dragvector));

		if (RatMathVector3GetMagnitude(perp))
		{
			// perpendicular vector as the transform
			rotq->x=perp->x;
			rotq->y=perp->y;
			rotq->z=perp->z;
			rotq->w=RatMathVector3Dot(&(arcball->grabvector),&(arcball->dragvector));
		}
		else
			memset(rotq,0,sizeof(RatMathQuaternion));
		
		free((void *)perp);
	}
}

void RatMathDumpMatrix4(RatMathMatrix4 *mat)
{
	printf("[[% 3.3f % 3.3f % 3.3f % 3.3f ]\n",mat->mat00,mat->mat10,mat->mat20,mat->mat30);
	printf(" [% 3.3f % 3.3f % 3.3f % 3.3f ]\n",mat->mat01,mat->mat11,mat->mat21,mat->mat31);
	printf(" [% 3.3f % 3.3f % 3.3f % 3.3f ]\n",mat->mat02,mat->mat12,mat->mat22,mat->mat32);
	printf(" [% 3.3f % 3.3f % 3.3f % 3.3f ]]\n",mat->mat03,mat->mat13,mat->mat23,mat->mat33);
}

void RatMathDumpMatrix4AsVectors(RatMathMatrix4 *mat)
{
	RatMathVector3 up,right,look,pos;
	RatMathMatrix4ExtractUpVector(mat,&up);
	RatMathMatrix4ExtractRightVector(mat,&right);
	RatMathMatrix4ExtractLookVector(mat,&look);
	RatMathMatrix4ExtractPosVector(mat,&pos);
	printf("Up:	");
	RatMathDumpVector3(&up);
	printf("Right: ");
	RatMathDumpVector3(&right);
	printf("Look:  ");
	RatMathDumpVector3(&look);
	printf("Pos:   ");
	RatMathDumpVector3(&pos);
}

void RatMathDumpVector3(RatMathVector3 *vec)
{
	printf("(% 3.3f,% 3.3f,% 3.3f)\n",vec->x,vec->y,vec->z);
}

void RatMathDumpEuler(RatMathEuler *eul)
{
	printf("(% 3.3f,% 3.3f,% 3.3f)\n",eul->x,eul->y,eul->z);
}

void RatMathDumpQuaternion(RatMathQuaternion *quat)
{
	printf("(% 3.3f,% 3.3f,% 3.3f,[% 3.3f])\n",quat->x,quat->y,quat->z,quat->w);
}

